#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from control_msgs.msg import GripperCommand
from geometry_msgs.msg import Pose
from copy import deepcopy
from math import pi

 # 角度转弧度
DE2RA = pi / 180.0

class ArmMover:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        # 初始化ROS节点
        rospy.init_node('moveit_cartesian_demo', anonymous=True)              
        # 初始化需要使用move group控制的机械臂中的arm group
        self.arm = MoveGroupCommander('gluon')
        # 初始化需要使用move group控制的机械臂中的gripper group
        self.gripper = moveit_commander.MoveGroupCommander('Gripper')
        
        # 当运动规划失败后，允许重新规划
        self.arm.allow_replanning(True)
        # 设置目标位置所使用的参考坐标系
        self.arm.set_pose_reference_frame('base_link')      
        
        # --ik----设置位置(单位：米)和姿态（单位：弧度）的允许误差
        self.arm.set_goal_position_tolerance(0.01)
        self.arm.set_goal_orientation_tolerance(0.1)
        
        # --fk----设置机械臂和夹爪的允许误差值
        self.arm.set_goal_joint_tolerance(0.001)
        self.gripper.set_goal_joint_tolerance(0.001)


        # 获取终端link的名称和起始位姿
        self.end_effector_link = self.arm.get_end_effector_link()

    def Gohome(self,vel=1):
        '''
        移动机械臂到home
        @ vel 移动速度 0-1之间
        '''
        # 控制机械臂运动到之前设置的“home”姿态
        self.arm.set_max_velocity_scaling_factor(vel)
        self.arm.set_named_target('home_pose')
        self.arm.go()

    def MovebyPose(self,target_pose,vel=1):
        '''
        移动机械臂
        @ target_pose 末端执行器目标位置xyz +四元数
        @ vel 移动速度 0-1之间
        '''
        self.arm.set_max_velocity_scaling_factor(vel)
        # 设置机器臂当前的状态作为运动初始状态
        self.arm.set_start_state_to_current_state()
        # 设置机械臂终端运动的目标位姿
        self.arm.set_pose_target(target_pose, self.end_effector_link)
        # 规划运动路径
        plan_success,traj,planning_time,error_code = self.arm.plan()
        # 按照规划的运动路径控制机械臂运动
        self.arm.execute(traj)
        rospy.sleep(0.5)
    
    def ShiftPose(self,axis,value,vel=1):
        '''
        调整当前pose值
        :axix  部位 0-5分别对应 xyz RPY
        :value  xyz 为m RPY为rad
        @ vel 移动速度 0-1之间
        '''
        self.arm.set_max_velocity_scaling_factor(vel)
        self.arm.shift_pose_target(axis, value, self.end_effector_link)
        self.arm.go()
        # rospy.sleep(1)

    def MovebyJoint(self,joints,vel=1):
        '''
        移动机械臂 无夹爪
        @ joints 6自由度 各关节角度 单位 度 如[0.0, 0.0, 0.0, 0.0, 0.0, 0,0]
        @ vel 移动速度 0-1之间
        '''

        for i in range(6):
            joints[i] = joints[i] * DE2RA

        self.arm.set_max_velocity_scaling_factor(vel)
        self.arm.set_joint_value_target(joints)     
        # 控制机械臂完成运动
        self.arm.go()
        # rospy.sleep(0.5)

    def ShiftGripperJoint(self,value,vel=1):
        '''
        移动机械臂夹爪 需要在urdf模型中弄好夹爪组
        @ value 关节角度 如[90] 度
        @ vel 移动速度 0-1之间
        '''
        value = value * DE2RA
        self.gripper.set_max_velocity_scaling_factor(vel)
        self.gripper.set_joint_value_target(value)
        self.gripper.go()
        rospy.sleep(1)

    def MovebysomePose(self,target_pose,interval_num = 1,vel=1):
        '''
        笛卡尔直线运动规划  待修改
        : target_pose 末端执行器目标位置
        : interval_num 间隔数目
        @ vel 移动速度 0-1之间
        '''
        self.arm.set_max_velocity_scaling_factor(vel)
        # 初始化位姿
        start_pose = self.arm.get_current_pose(self.end_effector_link).pose                               
        print(start_pose)

        # 中间位姿  从start_pose 到 target_pose
        wpose = deepcopy(start_pose)
        waypoints = []
        # waypoints.append(start_pose)

        print("1")
        ## 设置路点    
        interval_x = (target_pose.position.x - start_pose.position.x)/interval_num
        interval_y = (target_pose.position.y - start_pose.position.y)/interval_num
        interval_z = (target_pose.position.z - start_pose.position.z)/interval_num

        for i in range(interval_num):
            wpose.position.x += interval_x
            wpose.position.y += interval_y
            wpose.position.z += interval_z
            print(wpose)
            waypoints.append(wpose)

        fraction = 0.0   #路径规划覆盖率
        maxtries = 100   #最大尝试规划次数
        attempts = 0     #已经尝试规划次数
        
        # 设置机器臂当前的状态作为运动初始状态
        self.arm.set_start_state_to_current_state()
        print("2")

        # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
        while fraction < 1.0 and attempts < maxtries:
            i = 0
            print("ok",i)
            i = i + 1
            (plan, fraction) = self.arm.compute_cartesian_path (waypoints, 0.01, 0.0, True)         
            attempts += 1      
            if attempts % 10 == 0:
                self.rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
        
        print(3)  

        # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the arm.")
            self.arm.execute(plan)
            rospy.loginfo("Path execution complete.")
        # 如果路径规划失败，则打印失败信息
        else:
            self.rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

        rospy.sleep(1)
    
    def CloseControl(self):
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
